#ifndef DIRECT_NAVIGATION_DIRECT_NAVIGATOR_HPP_
#define DIRECT_NAVIGATION_DIRECT_NAVIGATOR_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include "direct_navigation/center_json.hpp"
#include "direct_navigation/vel_json.hpp"
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <jsoncpp/json/json.h>
#include <memory>
#include <vector>
#include <deque>
#include <cmath>
#include <chrono>

namespace direct_navigation {

    using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

    // 速度控制进程状态枚举
    enum ControlState {
        IDLE,           // 空闲状态
        SAMPLING_Y,     // X轴采样状态  
        MOVING_Y,       // X轴移动状态
        SAMPLING_X,     // Y轴采样状态
        MOVING_X,       // Y轴移动状态
        VERIFYING,      // 最终验证状态
        REACHED         // 已到达目标状态
    };

    // 样本数据
    struct Point3D {
        double x, y, z;
    };

    class DirectNavigator : public rclcpp_lifecycle::LifecycleNode {
    public:
        explicit DirectNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
        ~DirectNavigator() override = default;
    private:
        // 生命周期回调
        CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
        CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
        CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
        CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;

        // 参数声明
        void declare_parameters();

        // 回调函数
        void centerJsonCallback(const std_msgs::msg::String::SharedPtr msg);
        void controlLoop();

        // 状态处理函数
        void handleIdleState();
        void handleSamplingYState();
        void handleMovingYState();
        void handleSamplingXState();
        void handleMovingXState();
        void handleVerifyingState();
        void handleReachedState();

        // 核心功能
        void startSampling();
        void processSamples();
        std::pair<std::vector<double>, std::vector<double>> removeOutliers(
            const std::vector<double>& x_coords, const std::vector<double>& y_coords);
        void startMovingToTarget();
        void startMovingYToTarget();
        void stopRobot();
        void publishVelocity();
        double calculateMovementTime(double distance, double max_vel, double max_acc);
        void applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                    double target_linear_x, double target_linear_y, double target_angular_z);
        void publishReachedStatus(bool success);


        // 修改成员变量类型为 LifecyclePublisher
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr center_json_sub_;
        std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> cmd_vel_pub_;
        std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> reached_pub_;
        // 定时器
        rclcpp::TimerBase::SharedPtr control_timer_;

        std::string center_json_topic_;
        std::string cmd_vel_topic_;
        std::string reached_topic_;

        // json类实体
        CenterJsonConverter center_coords;
        VelocityJsonConverter vel_cmd;

        // 运动控制相关
        double max_linear_velocity_;
        double y_position_tolerance_;
        double x_position_tolerance_;
        int controlling_period_;
        double max_acceleration_;
        double max_angular_acceleration_;
        std::chrono::duration<double> sampling_duration_;
        double SIGMA_THRESHOLD_;

        // 状态变量
        double current_center_x_;
        double current_center_y_;
        double current_center_z_;
        bool target_detected_;
        ControlState control_state_;
        double avg_center_x_, avg_center_y_, avg_center_z_;
        rclcpp::Time sampling_start_time_;
        
        // 速度控制变量
        double linear_x;
        double linear_y;
        double angular_z;
        double target_linear_x;
        double target_linear_y; 
        double target_angular_z; 
        double prev_linear_x_;
        double prev_linear_y_;
        double prev_angular_z_;
        double movement_time_x_, movement_time_y_;
        double movement_timer_;

        // 样本点存储
        std::vector<Point3D> sample_points_;
        
        // 添加状态标志
        bool state_is_active_;
    };

} // namespace direct_navigation

#endif // DIRECT_NAVIGATION_DIRECT_NAVIGATOR_HPP_